#ifndef TRANSFORM_H
#define TRANSFORM_H
#include <eigen3/Eigen/Eigen>
#include <iostream>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/vector3.hpp>
#include <geometry_msgs/msg/quaternion.hpp>

namespace transform {
class Transform
{
public:
  using Vector = Eigen::Matrix<double, 3, 1>;
  using Quaternion = Eigen::Quaterniond;
  Transform() : translation_(Vector::Zero()), rotation_(Quaternion::Identity()) {}

  Transform(const Vector& translation, const Quaternion& rotation)
      : translation_(translation), rotation_(rotation) {}


  static Transform Identity() { return Transform(); }

  const Vector& translation() const { return translation_; }
  const Quaternion& rotation() const { return rotation_; }
  Eigen::Matrix4d matrix44();
  Eigen::Matrix3d matrix33();

  Transform inverse() const {
    const Quaternion rotation = rotation_.conjugate();
    const Vector translation = -(rotation * translation_);
    return Transform(translation, rotation);
  }

  Eigen::Quaterniond ToEigen(const geometry_msgs::msg::Quaternion& quaternion) {
    return Eigen::Quaterniond(quaternion.w, quaternion.x, quaternion.y,
                              quaternion.z);
  }

  void setTransform(const double& x,const double& y,const double& yaw);
  void setTransform(const geometry_msgs::msg::Pose& pose);

  Eigen::Quaterniond RollPitchYaw(const double roll, const double pitch,
                                  const double yaw);

  // Returns the yaw component in radians of the given 3D 'rotation'. Assuming
  // 'rotation' is composed of three rotations around X, then Y, then Z, returns
  // the angle of the Z rotation.
  double GetYaw(const Eigen::Quaterniond& rotation) {
    const Eigen::Matrix<double, 3, 1> direction =
        rotation * Eigen::Matrix<double, 3, 1>::UnitX();
    return atan2(direction.y(), direction.x());
  }

  double GetYaw() {
    const Eigen::Matrix<double, 3, 1> direction =
        rotation_ * Eigen::Matrix<double, 3, 1>::UnitX();
    return atan2(direction.y(), direction.x());
  }

  double Norm(){
    return sqrt(pow(translation_.x(),2)+pow(translation_.y(),2));
  }

  void setTransform(Eigen::Matrix4d RT);

  friend Transform operator*(const Transform& lhs,
                              const Transform& rhs) {//左右值
    return Transform(
        lhs.rotation() * rhs.translation() + lhs.translation(),
        (lhs.rotation() * rhs.rotation()).normalized());
  }
  
private:
  Vector translation_;
  Quaternion rotation_;

};
}
#endif // TRANSFORM_H
